#include <stdio.h>
#include <unistd.h>
#include "cmsis_os2.h"
#include "ohos_init.h"
#include "iot_gpio.h"
#include "iot_i2c.h"
#include "hi_io.h"

// I2C配置参数
#define I2C_IDX 1           // 使用I2C1控制器
#define I2C_BAUDRATE 400000 // 400kHz标准模式
#define MPU6050_ADDR 0x68   // AD0接地时的设备地址

// MPU6050寄存器定义
#define PWR_MGMT_1 0x6B
#define ACCEL_CONFIG 0x1C
#define CONFIG 0x1A
#define WHO_AM_I 0x75
#define ACCEL_XOUT_H 0x3B

// 初始化MPU6050
void mpu6050_init()
{

    IoTI2cDeinit(I2C_IDX);
    // 初始化引脚
    IoTGpioInit(HI_IO_NAME_GPIO_3);
    IoTGpioInit(HI_IO_NAME_GPIO_4);

    // 配置GPIO13和GPIO14复用为I2C功能
    hi_io_set_func(HI_IO_NAME_GPIO_3, HI_IO_FUNC_GPIO_3_I2C1_SDA);
    hi_io_set_func(HI_IO_NAME_GPIO_4, HI_IO_FUNC_GPIO_4_I2C1_SCL);

    
    // 初始化I2C控制器
    uint16_t retval = IoTI2cInit(I2C_IDX, I2C_BAUDRATE);
    printf("I2C Init Return: %d\n", retval);

    // IoTI2cSetBaudrate(I2C_IDX,I2C_BAUDRATE);

    // 复位设备（PWR_MGMT_1寄存器写0x80）
    uint8_t reset_cmd[] = {PWR_MGMT_1, 0x80};
    IoTI2cWrite(I2C_IDX, MPU6050_ADDR << 1, reset_cmd, sizeof(reset_cmd));
    osDelay(2);

    // 唤醒设备（PWR_MGMT_1寄存器写0x00）
    uint8_t wakeup_cmd[] = {PWR_MGMT_1, 0x00};
    IoTI2cWrite(I2C_IDX, MPU6050_ADDR << 1, wakeup_cmd, sizeof(wakeup_cmd));
    osDelay(2);

    // 配置加速度计量程为±8g（ACCEL_CONFIG寄存器）
    uint8_t accel_config[] = {ACCEL_CONFIG, 0x10}; // 0x10对应±8g
    IoTI2cWrite(I2C_IDX, MPU6050_ADDR << 1, accel_config, sizeof(accel_config));
    osDelay(2);

    // 配置数字低通滤波器（可选）
    uint8_t dlpf_config[] = {CONFIG, 0x03}; // 44Hz带宽
    IoTI2cWrite(I2C_IDX, MPU6050_ADDR << 1, dlpf_config, sizeof(dlpf_config));
    osDelay(2);

    // 验证设备ID
    uint8_t dev_id = 0;
    uint8_t reg = WHO_AM_I;
    IoTI2cWrite(I2C_IDX, MPU6050_ADDR << 1, &reg, 1); // 发送寄存器地址
    osDelay(2);
    IoTI2cRead(I2C_IDX, MPU6050_ADDR << 1, &dev_id, 1); // 读取数据
    printf("WHO_AM_I: 0x%02X\n", dev_id);
}

// 读取加速度计数据
void read_mpu6050(float *x,float *y,float *z)
{
    uint8_t reg_addr = ACCEL_XOUT_H;
    uint8_t buffer[6];

    // 写入要读取的起始寄存器地址
    IoTI2cWrite(I2C_IDX, MPU6050_ADDR << 1, &reg_addr, 1);
    osDelay(2);
    // 连续读取6个字节（X/Y/Z各2字节）
    IoTI2cRead(I2C_IDX, MPU6050_ADDR << 1, buffer, sizeof(buffer));
    osDelay(2);
    // 数据转换（注意高位在前）
    int16_t accel_x = (buffer[0] << 8) | buffer[1];
    int16_t accel_y = (buffer[2] << 8) | buffer[3];
    int16_t accel_z = (buffer[4] << 8) | buffer[5];

    // 转换为g值（±8g量程对应4096 LSB/g）
    float g_x = accel_x / 4096.0f;
    float g_y = accel_y / 4096.0f;
    float g_z = accel_z / 4096.0f;

    *x=g_x;
    *y=g_y;
    *z=g_z;

    printf("G值: X=%.2f, Y=%.2f, Z=%.2f\n", g_x, g_y, g_z);

}
